import socket
import struct
import math
import csv
import os
import time
from vjoy_control_new import revive_control  # 导入复飞控制函数

# 其他文件可以通过导入 get_dcs_data 函数来调用本文件
# from getDCSdata import get_dcs_data
# get_dcs_data(udp_ip="127.0.0.1", udp_port=12345)

# 定义超时时间（秒）
DATA_TIMEOUT = 3.0


def get_dcs_data(udp_ip="127.0.0.1", udp_port=12345):
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind((udp_ip, udp_port))
    sock.settimeout(DATA_TIMEOUT)  # 设置socket超时

    # 修正后的预期数据长度（与C++的bufferSize一致）
    expected_length = 264  # 276 - 12 = 264
    print(f"[Python] Expected data length: {expected_length} bytes")

    # 定义 CSV 文件名
    csv_filename = "results//getDCSdata.csv"

    # 检查文件是否存在，如果不存在则创建并写入表头
    file_exists = os.path.exists(csv_filename)
    with open(csv_filename, 'a', newline='') as csvfile:
        writer = csv.writer(csvfile)
        if not file_exists:
            # 写入表头
            writer.writerow([
                "ID", "targetID", "gunID", "flags", "M_", "IAS", "heading_deg", "AoA",
                "Point_X", "Point_Y", "Point_Z", "VV_X", "VV_Y", "VV_Z", "VV_0",
                "ACC_X", "ACC_Y", "ACC_Z",
                "X_Axis_X", "X_Axis_Y", "X_Axis_Z",
                "Y_Axis_X", "Y_Axis_Y", "Y_Axis_Z",
                "Z_Axis_X", "Z_Axis_Y", "Z_Axis_Z",
                "Omega_X", "Omega_Y", "Omega_Z",
                "pitch_deg", "bank_deg", "yaw_deg",
                "LLA_long", "LLA_lat", "LLA_balt",
                "Gears", "Flaps", "AirBrake", "Fuel",
                "RPM_0", "RPM_1", "RPM_2", "RPM_3", "RPM_4", "RPM_5"
            ])

    last_data_time = time.time()  # 记录最后一次收到数据的时间

    while True:
        try:
            data, addr = sock.recvfrom(65536)
            last_data_time = time.time()  # 更新最后接收时间

            if len(data) != expected_length:
                print(f"Invalid data length: {len(data)} bytes (expected {expected_length})")
                continue

            try:
                offset = 0

                # --- 解析字段（与C++顺序严格一致）---
                # 1. 基础信息（4个uint32_t）
                ID = struct.unpack_from('I', data, offset)[0];
                offset += 4
                targetID = struct.unpack_from('I', data, offset)[0];
                offset += 4
                gunID = struct.unpack_from('I', data, offset)[0];
                offset += 4
                flags = struct.unpack_from('i', data, offset)[0];
                offset += 4
                hex_flags = hex(flags)  # 转为16进制

                # 2. 运动参数（4个float）
                M_ = struct.unpack_from('f', data, offset)[0];
                offset += 4
                IAS = struct.unpack_from('f', data, offset)[0];
                offset += 4  # 指示空速
                heading = struct.unpack_from('f', data, offset)[0];
                offset += 4
                AoA = struct.unpack_from('f', data, offset)[0];
                offset += 4

                # 3. 位置、速度、加速度（各3个double）
                Point = struct.unpack_from('3d', data, offset);
                offset += 24
                VV = struct.unpack_from('3d', data, offset);
                offset += 24
                VV_0 = math.sqrt(VV[0] * VV[0] + VV[1] * VV[1] + VV[2] * VV[2])  # 对地速度  新增
                ACC = struct.unpack_from('3d', data, offset);
                offset += 24

                # 4. 三轴矢量（各3个double）
                X_Axis = struct.unpack_from('3d', data, offset);
                offset += 24
                Y_Axis = struct.unpack_from('3d', data, offset);
                offset += 24
                Z_Axis = struct.unpack_from('3d', data, offset);
                offset += 24

                # 5. 角速度（3个double）
                Omega = struct.unpack_from('3d', data, offset);
                offset += 24

                # 6. 姿态角（3个float）
                pitch = struct.unpack_from('f', data, offset)[0];
                offset += 4
                bank = struct.unpack_from('f', data, offset)[0];
                offset += 4
                yaw = struct.unpack_from('f', data, offset)[0];
                offset += 4

                # 7. 经纬高（3个float，原错误解析为double）
                LLA_long = struct.unpack_from('f', data, offset)[0];
                offset += 4
                LLA_lat = struct.unpack_from('f', data, offset)[0];
                offset += 4
                LLA_balt = struct.unpack_from('f', data, offset)[0];
                offset += 4
                # 确定经度和纬度的符号
                long_sign = 'E' if LLA_long > 0 else 'W'  # 经度
                lat_sign = 'N' if LLA_lat > 0 else 'S'  # 纬度

                # 8. 机械状态（4个float）
                Gears = struct.unpack_from('f', data, offset)[0];
                offset += 4
                Flaps = struct.unpack_from('f', data, offset)[0];
                offset += 4
                AirBrake = struct.unpack_from('f', data, offset)[0];
                offset += 4
                Fuel = struct.unpack_from('f', data, offset)[0];
                offset += 4

                # 9. RPM（6个float）
                RPM = []
                for _ in range(6):
                    rpm_value = struct.unpack_from('f', data, offset)[0]
                    RPM.append(rpm_value)
                    offset += 4

                # --- 格式化RPM输出 ---
                # 前两个RPM值保留四位有效数字，其余保持原样
                formatted_RPM = [f"{rpm:.4g}" if i < 2 else f"{rpm:.4f}" for i, rpm in enumerate(RPM)]

                # --- 弧度转换为度 ---
                heading_deg = math.degrees(heading)  # 真航向
                pitch_deg = math.degrees(pitch)  # 俯仰角
                bank_deg = math.degrees(bank)  # 滚转角
                yaw_deg = math.degrees(yaw)  # 偏航角

                # --- 将数据写入 CSV 文件 ---
                with open(csv_filename, 'a', newline='') as csvfile:
                    writer = csv.writer(csvfile)
                    writer.writerow([
                        ID, targetID, gunID, hex_flags, M_, IAS, heading_deg, AoA,
                        Point[0], Point[1], Point[2], VV[0], VV[1], VV[2], VV_0,
                        ACC[0], ACC[1], ACC[2],
                        X_Axis[0], X_Axis[1], X_Axis[2],
                        Y_Axis[0], Y_Axis[1], Y_Axis[2],
                        Z_Axis[0], Z_Axis[1], Z_Axis[2],
                        Omega[0], Omega[1], Omega[2],
                        pitch_deg, bank_deg, yaw_deg,
                        LLA_long, LLA_lat, LLA_balt,
                        Gears, Flaps, AirBrake, Fuel,
                        RPM[0], RPM[1], RPM[2], RPM[3], RPM[4], RPM[5]
                    ])

                # 返回解析后的数据
                return {
                    "ID": ID,
                    "targetID": targetID,
                    "gunID": gunID,
                    "flags": hex_flags,
                    "M_": M_,
                    "IAS": IAS,
                    "heading_deg": heading_deg,
                    "AoA": AoA,
                    "Point": Point,
                    "VV": VV,
                    "VV_0": VV_0,
                    "ACC": ACC,
                    "X_Axis": X_Axis,
                    "Y_Axis": Y_Axis,
                    "Z_Axis": Z_Axis,
                    "Omega": Omega,
                    "pitch_deg": pitch_deg,
                    "bank_deg": bank_deg,
                    "yaw_deg": yaw_deg,
                    "LLA_long": LLA_long,
                    "LLA_lat": LLA_lat,
                    "LLA_balt": LLA_balt,
                    "Gears": Gears,
                    "Flaps": Flaps,
                    "AirBrake": AirBrake,
                    "Fuel": Fuel,
                    "RPM": RPM
                }

            except struct.error as e:
                print(f"Parse error: {e}")

        except socket.timeout:
            # 检查是否超时
            if time.time() - last_data_time > DATA_TIMEOUT:
                print(f"[WARNING] 超过{DATA_TIMEOUT}秒未收到数据，发送复飞命令")
                try:
                    revive_control()  # 发送复飞命令
                    last_data_time = time.time()  # 重置计时器
                except Exception as e:
                    print(f"[ERROR] 发送复飞命令失败: {e}")
            continue


# 如果直接运行此脚本，则启动数据接收
if __name__ == "__main__":
    get_dcs_data()